#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "turtlesim/msg/pose.hpp"
using turtlesim::msg::Pose;
using std::placeholders::_1;
class TFBroadcaster:public rclcpp::Node
{
public:
    TFBroadcaster():Node("tf_broadcaster_node")
    {
        RCLCPP_INFO(this->get_logger(),"动态坐标发布节点已创建！");
        tb_=std::make_shared<tf2_ros::TransformBroadcaster>(this);
        subsrciption_=this->create_subscription<Pose>
        ("/turtle1/pose",
        10,
        std::bind(&TFBroadcaster::get_transform,this,_1)
        ); 
    }
private:    
    void get_transform(const Pose &pose)
    {
        auto ts=geometry_msgs::msg::TransformStamped();
        ts.header.stamp=this->get_clock()->now();
        ts.header.frame_id="world";
        ts.child_frame_id="turtle";
        ts.transform.translation.x=pose.x;
        ts.transform.translation.y=pose.y;
        ts.transform.translation.z=0;

        tf2:: Quaternion qtn;
        qtn.setRPY(0,0,pose.theta);  
        //ts.transform.rotation=tf2::toMsg(qtn) 字样可以一次性转完
        ts.transform.rotation.x=qtn.x();
        ts.transform.rotation.y=qtn.y();
        ts.transform.rotation.z=qtn.z();
        ts.transform.rotation.w=qtn.w();
        tb_->sendTransform(ts);
    }
private:
    std::shared_ptr<tf2_ros::TransformBroadcaster>tb_;
    std::shared_ptr<rclcpp::Subscription<Pose>>subsrciption_;   
};
int main(int argc,char*argv[])
{
    rclcpp::init(argc,argv);
    auto node=std::make_shared<TFBroadcaster>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}